#include "Wire.h"
#include "Driver.h"
#include "Arduino.h"

Driver::Driver(uint8_t address /*= 0x5f*/)
{
	m_address = address;
}

void Driver::begin()
{
	Wire.begin();
	reset();
}

void Driver::reset(void)
{
	write8(MODE1, 0x0);
}

void Driver::setFreq(float freq)
{
	float prescaleval = 25000000;
	prescaleval /= 4096;
	prescaleval /= freq;
	prescaleval -= 1;
	uint8_t prescale = floor(prescaleval + 0.5);
  
	uint8_t oldmode = read8(MODE1);
	uint8_t newmode = (oldmode & 0x7F) | 0x10; // sleep
	write8(MODE1, newmode); // go to sleep
	write8(PRESCALE, prescale); // set the prescaler
	write8(MODE1, oldmode);
	delay(5);
	write8(MODE1, oldmode | 0x80);
}

void Driver::setLevel(uint8_t num, uint16_t on, uint16_t off)
{
	write8(LED0_ON_L+4*num, on);
	write8(LED0_ON_H+4*num, on >> 8);
	write8(LED0_OFF_L+4*num, off);
	write8(LED0_OFF_H+4*num, off >> 8);  
}

uint8_t Driver::read8(uint8_t addr)
{
	Wire.beginTransmission(m_address);
	Wire.write(addr);
	Wire.endTransmission();
	Wire.requestFrom(m_address, (uint8_t)1);
	return Wire.read();
}

void Driver::write8(uint8_t addr, uint8_t d)
{
	Wire.beginTransmission(m_address);
	Wire.write(addr);
	Wire.write(d);
	Wire.endTransmission();
}
